/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * Description: Global definitions and configurations
 *         $Id: car.h 49 2014-05-01 21:06:14Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#ifndef _CAR_H_
#define _CAR_H_

#define NUM_WHEELS  2

#define LEFT        0
#define RIGHT       1
#define CENTER      2

#define FORWARD     0
#define REVERSE     1

// Modes
typedef enum {
  FIXED_VALUE = 0,
  FIXED_SPEED = 1,
  TURNING = 2,
  COASTING = 3,
  PAUSE = 4
} op_mode_t;

#define SPEED_LOG_SIZE    60
#define DIST_LOG_SIZE     50

#define TRIP_DEST_INIT    100

#define NUM_OBJ_SENSORS   3
#define MIN_DISTANCE      40

// d = 42 mm, c = pi * d = 131.947 mm
#define WHEEL_CIRCUM_10MM_LEFT    1338  // 10 * mm
#define WHEEL_CIRCUM_10MM_RIGHT   1327  // 10 * mm
#define WHEEL_CIRCUM_MMS4_LEFT    2141  // mm * 16
#define WHEEL_CIRCUM_MMS4_RIGHT   2123  // mm * 16

#define START_VALUE_LEFT    0x2C
#define START_VALUE_RIGHT   0x29

#define WHEELBASE_MM    107   // mm

#if (WHEELBASE_MM == 107)
#define DIST_ROTATE_90_DEG    84
#define DIST_ROTATE_180_DEG   168
#define DIST_ROTATE_360_DEG   336
#endif
#define VALUE_ROTATE  0x25

#define TURN_RADIUS   547
//#define TURN_RADIUS   500
//#define TURN_RADIUS   300

#if (TURN_RADIUS == 500)

#if (WHEELBASE_MM == 107)
#define ARC_INSIDE  785   // mm
#define ARC_OUTSIDE 953   // mm
#define in_whl_spd(setpoint)   ((setpoint * 40) / 49)
#else
#error Unsupported WHEELBASE_MM
#endif

#elif (TURN_RADIUS == 300)

#if (WHEELBASE_MM == 107)
#define ARC_INSIDE  471   // mm
#define ARC_OUTSIDE 639   // mm
#define in_whl_spd(setpoint)   ((setpoint * 25) / 34)
#else
#error Unsupported WHEELBASE_MM
#endif

#elif (TURN_RADIUS == 547)
#if(WHEELBASE_MM == 107)
#define ARC_INSIDE 859
#define ARC_OUTSIDE 1027
#define in_whl_spd(setpoint) ((setpoint * 10) / 12)
#endif
#else
#error Unsupported turning radius
#endif

#define TURN_LAT_DIST   (TURN_RADIUS + ((WHEELBASE_MM + 1) / 2))

void print_i32(int32_t value);

#define calc_odometer_value(wheel, value) ((value * hundred_mm_per_cnt[wheel]) / 100)

#define DIST_BETWEEN_SENSORS        15  // mm
#define wall_dist(width)            ((width - DIST_BETWEEN_SENSORS) / 2)
#define wall_width(distl, distr)    (distl + distr + DIST_BETWEEN_SENSORS)

// Timer Assignments
#define TMR_ENC_L     MXC_TMR0
#define TMR_MOTOR_L   MXC_TMR1
#define TMR_ENC_R     MXC_TMR2
#define TMR_MOTOR_R   MXC_TMR3

#endif /* _CAR_H_ */
